/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/***************************************************************************

 ROUTINE
               Smooth

 FUNCTION
                
               Smooth parameters when error conditions indicate it is necessary

 SYNOPSIS
               Smooth(assure, frame, subframe, parameters)

   formal 

                       data      I/O
       name            type      type    function
       -------------------------------------------------------------------
       assure		int	  i	EDAC assurance levels
       frame    	int	  i	frame number
       subframe 	int	  i	subframe number
       prameters        TX_PARAM i/o    array of parameters to be smoothed

==========================================================================

 DESCRIPTION

     Smooth parameters based on error level indications.  If edac is set
then smoothing is done according to the edac design.  If edac is not set
then smoothing is done according to channel estimates received from the
Hamming decoder.  A running average of the number of times the Hamming
codeword has received an error.  If this number is above a given threshold
then smoothing is performed.

**************************************************************************/
#include "main.h"
#include "smooth.h"
#include "variance.h"
#include "rint.h"

#define SYNDRUN 100.0
#define SYNDERR 0.01
#define HISTORY 4
#define SGAINHIGH 64.0
#define SGAINLOW -64.0
#define FRAME_MARK 4

void Smooth(
int       assure,
int       frame,
int       subframe,
TX_PARAM  *parameters)
{ float        syndrome;
  static float *AdaptGains;
  static float PDelay3;
  static float *gains;
  static float syndavg;

  if (subframe == 0) {
    if (assure == 0)
      syndrome = 0.0;
    else
      syndrome = 1.0;
    syndavg = ((1.0 - SYNDERR) * syndavg) + (SYNDERR * syndrome);
  }
  PDelay3 = parameters->AdaptiveDelay[2];
  AdaptGains = parameters->AdaptiveGain;
  gains = parameters->StochasticGain;

  smoothPDelay(&parameters->AdaptiveDelay[subframe], syndavg, PDelay3, frame, subframe+1);
  smoothAdaptGain(&parameters->AdaptiveGain[subframe], syndavg, AdaptGains, frame, subframe+1);
  smoothStochGain(&parameters->StochasticGain[subframe], syndavg, gains, frame, subframe+1);

}

/*

**************************************************************************

 ROUTINE
               smoothPDelay

 FUNCTION
                
               smooth PDelay values

 SYNOPSIS
               smoothPDelay(PDelay,syndavg,PDelay3,subframe,frame)

   formal 

                       data    I/O
       name            type    type    function
       -------------------------------------------------------------------
       PDelay		real	i/o	input PDelay
       syndavg		real	i	error rate estimation parameter
       PDelay3		float	i	third PDelay value
       frame    	int	i	frame number
       subframe 	int	i	subframe number

==========================================================================

 DESCRIPTION

	Routine to smooth PDelay (pitch lag) when errors are detected:

	If the variance of past PDelay values is within the range VARLIMIT 
	(indicating voiced speech) the validity of the current PDelay value
	is tested. If the current value of PDelay is within the range
	PDELAYLIMIT, PDELAY is passed. If PDELAY is not within the range
	PDELAYLIMIT, PDELAY is reset to the average value of PDelays.
 
	The array OLDPDELAY contains past values of PDelay.  The array VECTOR
	is constructed from the array OLDPDELAY and PDELAY3 for subframes 1
	and 2 (PDELAY3 is a future absolute PDelay value).  For subframes 3
	and 4 there are no valid future values (since delta PDelays in the 
	future are not valid), therefore the array VECTOR is constructed 
	entirely from the array OLDPDELAY.  Decisions concering smoothing of 
	a particular PDelay are made on the variance of the array VECTOR and 
	the PDelay in question (PDELAY).

	If the value of PDelay is smoothed in subframe 3, smoothing is disabled
	for subframe 4 of the same frame since the PDelay value in subframe 4
	is a delta based on subframe 3.

	Note:  The smoothing parameters should be capable of adapting to
	various bit error rate estimates. For example, different values
	of SYNDAVG should select different levels of PDELAYLIMIT and VARLIMIT.

**************************************************************************/

#define PDLIMIT   15
#define PDVARLIMIT   15
#define SYNDLIMIT  0.01

void smoothPDelay(
float  *PDelay, 
float  syndavg,
float  PDelay3,
int    frame, 
int    subframe)
{ int i;
  static int enable;
  float avg, var, vector[4];
  static float oldPDelay[HISTORY];

  if (subframe != 4) enable = 1;
  if ((syndavg > SYNDLIMIT) && enable) {
    switch (subframe) {
      case 1: case 2:
      vector[0]=oldPDelay[0];      vector[1]=oldPDelay[1];
      vector[2]=oldPDelay[2];      vector[3]=PDelay3;
      break;

      case 3: case 4:
      vector[0]=oldPDelay[0];      vector[1]=oldPDelay[1];
      vector[2]=oldPDelay[2];      vector[3]=oldPDelay[3];
      break;

      default:
      fprintf(stderr,"smoothPDelay: Error in subframe number: %d\n", subframe);
      break;
    }
    variance(vector, 4, &var, &avg);
    if ((var < PDVARLIMIT)&&((*PDelay>(avg+PDLIMIT))||(*PDelay<(avg-PDLIMIT)))) {
      *PDelay = rint(avg);
      fprintf(stderr,"smoothPDelay: Pitch Delay value set to %g. frame %d subframe %d\n",
	      avg, frame, subframe);
      if (subframe == 3) {
	enable = 0;
	fprintf(stderr,"smoothPDelay: Pitch Delay smoothing disabled for subframe 4\n");
      }
    }    /** if var and *PDelay **/
  }      /** if (syndavg > SYNDLIMIT) && enable **/
  for (i = HISTORY-1; i > 0; i--)
    oldPDelay[i] = oldPDelay[i-i];
  oldPDelay[0] = *PDelay;

}


/*
**************************************************************************

 ROUTINE
               smoothStochGain

 FUNCTION
                
               smooth StochGain values

 SYNOPSIS
               smoothStochGain(StochGain,syndavg,gains,frame,subframe)

   formal 

                       data    I/O
       name            type    type    function
       -------------------------------------------------------------------
       StochGain	real	i/o	input StochGain
       syndavg		real	i	error rate estimation parameter
       gains		real	i	vector of gains to calculate variance
       frame    	int	i	frame number
       subframe 	int	i	subframe number

==========================================================================

 DESCRIPTION

	Smoothing routine to smooth StochGain when errors are detected:

	If the variance of past StochGain values is within the range VARLIMIT, 
	the validity of the current StochGain value is tested.  If the current 
	value of StochGain is within the range STOCHGAINLIMIT, STOCHGAIN is
	passed.  If STOCHGAIN is not within the range STOCHGAINLIMIT it is
	reset to the average value of the surrounding StochGain values.

	The array OLDSTOCHGAIN contains past values of StochGain.  The array
	GAINS contains current and future values of StochGain.  The array
	VECTOR is constructed from the arrays OLDSTOCHGAIN and GAINS
	depending on the current subframe.  STOCHGAIN is smoothed based on
	the statistics of VECTOR, which contains the nearest four 
	surrounding StochGain values, both past and future values, except
	where future values are not available (subframes 3 and 4).

	Note:  The smoothing parameters should be capable of adapting to
	various bit error rate estimates. For example, different values of
	SYNDAVG should select different levels of STOCHGAINLIMIT, VARLIMIT,
	and SYNDLIMIT.

**************************************************************************/

#define STOCHGAINLIMIT   300.0
#define CBVARLIMIT      30000.0
#define SGAINLIMIT    9.0
#define SVARLIMIT     10.0
#define AVGLIMIT      6.0
#define SYNDLIMIT     0.01

void smoothStochGain(
float *StochGain,
float syndavg,
float *gains,
int   frame,
int   subframe)
{ int i;
  float avg, var,sign, absStochGain, vector[4];
  static int enable;
  static float oldStochGain[HISTORY];

  absStochGain = fabs(*StochGain);
  if (subframe != 4) enable = 1;
  if ((syndavg > SYNDLIMIT) &&  enable) {
    switch (subframe) {
      case 1:
      vector[0]=oldStochGain[1];      vector[1]=oldStochGain[0];
      vector[2]=fabs(gains[1]);       vector[3]=fabs(gains[2]);
      break;

      case 2:
      vector[0]=oldStochGain[1];      vector[1]=oldStochGain[0];
      vector[2]=fabs(gains[2]);       vector[3]=fabs(gains[3]);
      break;

      case 3:
      vector[0]=oldStochGain[2];      vector[1]=oldStochGain[1];
      vector[2]=oldStochGain[0];      vector[3]=fabs(gains[3]);
      break;

      case 4:
      vector[0]=oldStochGain[3];      vector[1]=oldStochGain[2];
      vector[2]=oldStochGain[1];      vector[3]=oldStochGain[0];
      break;

      default:
      fprintf(stderr,"smoothStochGain: Error in subframe number: %d\n", subframe);
      break;
    }    /** end switch on subframe **/
    variance(vector, 4, &var, &avg);
    if (*StochGain < 0)   sign = -1.0;
    else                  sign = 1.0;
    if ((var < CBVARLIMIT) && ((absStochGain > (avg+STOCHGAINLIMIT)) || (absStochGain < (avg-STOCHGAINLIMIT)))) {
      absStochGain = avg;

      fprintf(stderr,"smoothStochGain:  StochGain value reset to %g StochGains at frame %d subframe %d\n", avg, frame, subframe);
    
      *StochGain = sign * absStochGain;
      if (subframe == 2) {
	enable = 0;
	fprintf(stderr,"smoothStochGain:  smoothing disabled for subframe 3\n");
      }
    }
    if ((var < SVARLIMIT) && (absStochGain > SGAINLIMIT) && (avg < AVGLIMIT) && enable) { 
      absStochGain = avg;
      *StochGain = sign * absStochGain;

      fprintf(stderr,"smoothStochGain:  StochGain value reset to %g (silence?) at frame %d subframe %d\n", avg, frame, subframe);

      if (subframe == 3) {
	enable = 0;
	fprintf(stderr,"smoothStochGain:  smoothing disabled for subframe 4\n");
      }    /** end if subframe 3 **/
    }      /** end if var < ... **/
  }        /** end if syndavg > && enable **/

  for (i = HISTORY-1; i >= 1; i--)
    oldStochGain[i] = oldStochGain[i-1];
  
  oldStochGain[0] = *StochGain;   /** was absStochGain **/
}


/*

**************************************************************************
 ROUTINE
               smoothAdaptGain
 FUNCTION
               smooth AdaptGain value
 SYNOPSIS
               smoothAdaptGain(AdaptGain,syndavg,AdaptGains,frame,subframe)
   formal 
                       data    I/O
       name            type    type    function
       -------------------------------------------------------------------
       AdaptGain	float	i/o	AdaptGain for current subframe
       syndavg		float	i	error rate estimation parameter
       AdaptGains	float	i	AdaptGains for current frame
       frame    	int	i	frame number
       subframe 	int	i	subframe number

==========================================================================

 DESCRIPTION

	Routine to smooth AdaptGain (alpha) when errors are detected:

	Due to the range of ADAPTGAIN, statistical variance is not appropriate.
	Pseudovariance is used and calculated as: 
		sum of delta oldAdaptGains/# of deltas

	If this variance of past AdaptGain values is within the range VARLIMIT,
	the validity of the current AdaptGain value is tested.  If the current 
	value of AdaptGain is within the range ADAPTGAINLIMIT, ADAPTGAIN is 
	passed. If ADAPTGAIN is not within that range it is reset to the
	average	value of surrounding AdaptGain values.

	The array OLDADAPTGAIN contains past values of AdaptGain.  The array
	ADAPTGAINS contains current and future values of AdaptGain.  The array 
	VECTOR is constructed from the arrays OLDADAPTGAIN and ADAPTGAINS 
	depending on the current subframe.  ADAPTGAIN is smoothed based on 
	the statistics of VECTOR, which contains the nearest four 
	surrounding AdaptGain values, both past and future values, except 
	where future values are not available (subframes 3 and 4).

	Absolute values of AdaptGain are used in averaging and reassigning
	AdaptGain.  All reassigned AdaptGains are limited to the range 0.0-1.0.

	Note:  The smoothing parameters should be capable of adapting to
	various bit error rate estimates. For example, different values of
	SYNDAVG should select different levels of ADAPTGAINLIMIT, VARLIMIT,
	and SYNDLIMIT.

**************************************************************************/

#define AGLIMIT   0.9
#define AGVARLIMIT     0.2
#define SYNDLIMIT    0.01

void smoothAdaptGain(
float  *AdaptGain, 
float  syndavg,
float  *AdaptGains,
int    frame, 
int    subframe)
{ int i;
  float AGain, avg, var, sum1 = 0.0, sum2 = 0.0, vector[4];
  static float oldAdaptGain[HISTORY];
  static int enable;

  AGain = *AdaptGain;
  if (subframe != 4) enable = 1;
  if ((syndavg > SYNDLIMIT) && enable) {
    switch (subframe) {
      case 1:
      vector[0] = oldAdaptGain[1];      vector[1] = oldAdaptGain[0];
      vector[2] = AdaptGains[1];        vector[3] = AdaptGains[2];
      break;

      case 2:
      vector[0] = oldAdaptGain[1];      vector[1] = oldAdaptGain[0];
      vector[2] = AdaptGains[2];        vector[3] = AdaptGains[3];
      break;

      case 3:
      vector[0] = oldAdaptGain[2];      vector[1] = oldAdaptGain[1];
      vector[2] = oldAdaptGain[0];      vector[3] = AdaptGains[3];
      break;

      case 4:
      vector[0] = oldAdaptGain[3];      vector[1] = oldAdaptGain[2];
      vector[2] = oldAdaptGain[1];      vector[3] = oldAdaptGain[0];
      break;

      default:
      fprintf(stderr,"smoothAdaptGain:  Error in subframe numbering: %d\n",
	      subframe);
      break;
    }
    for (i=0; i < HISTORY; i++)
      sum1 += vector[i];
    for (i = 0; i < HISTORY-1; i++)
      sum2 += fabs(vector[i] - vector[i+1]);

    avg = sum1 / (float) HISTORY;
    var = sum2 / (float) (HISTORY-1);
/**
    fprintf(stderr,"\t%d\t%2.4f\t%2.4f\n", subframe, avg, var);
**/
    if ((var < AGVARLIMIT) && enable) {
      if ((AGain > (avg + AGLIMIT)) || (AGain < (avg - AGLIMIT))) {
	if (avg > 1.0)      avg = 1.0; /* clip avg if:  avg > 1.0 */
	else if (avg < 0.0) avg = 0.0; /* or            0 < avg   */
	*AdaptGain = avg;
	fprintf(stderr,"smoothAdaptGain: AdaptGain value set to %g. frame %d subframe %d\n",
		avg, frame, subframe);
	if (subframe == 3) {
	  enable = 0;
	  fprintf(stderr,"smoothAdaptGain: smoothing disabled for subframe 4\n");
	}    /** subframe next to last and updated **/
      }      /** end absAdaptGain not within ADAPTGAINLIMIT of avg **/
    }        /** end (var < PGVARLIMIT) && enable **/
  }          /** end (syndavg > SYNDLIMIT) && enable **/
  for (i = HISTORY-1; i > 0; i--)
    oldAdaptGain[i] = oldAdaptGain[i-1];
  oldAdaptGain[0] = *AdaptGain;  /** was absAdaptGain **/
}
